import cv2
import numpy as np
import math
from pylab import *
np.set_printoptions(suppress=True)

def cal3dCoordinateFromPixelCoordinates(uv_l, uv_r):
    '''
    作者：
    日期：2022.1.18；
    功能：已知空间中一点P在双目相机左、右相机像素坐标系中的像素坐标坐标uv_l, uv_r，
        求解点P在世界坐标系下的三维坐标；
    输入：点P在左相机像素坐标系下的像素坐标坐标uv_l（u1,v1）;
         点P在左相机像素坐标系下的像素坐标坐标uv_r（u2,v2）;
    输出：P在世界坐标系下的三维坐标;
    DEMO：
            import numpy as np

            uv_l = np.array([3.55550593 / 0.0053, 2.65832473 / 0.0053])
            uv_r = np.array([6.39255778 / 0.0053, 2.65832473 / 0.0053])

            wp_point = cal3dCoordinateFromPixelCoordinates(uv_l, uv_r)
    '''
    mtx_l = np.array([[8 / 0.0053, 0, 640, 0],
                      [0, 8 / 0.0053, 512, 0],
                      [0, 0, 1, 0]])
    mtx_r = np.array([[8 / 0.0053, 0, 640, 0],
                      [0, 8 / 0.0053, 512, 0],
                      [0, 0, 1, 0]])

    R_lr = np.array([[1, 0, 0],
                     [0, 1, 0],
                     [0, 0, 1]])
    T_lr = np.array([[-145],
                     [0],
                     [0]])

    cameraMatrixL = mtx_l[:, 0:3]
    cameraMatrixR = mtx_r[:, 0:3]

    # 左相机到左相机的投影矩阵
    R_ll = ([[1, 0, 0],
             [0, 1, 0],
             [0, 0, 1]])
    T_ll = ([[0], [0], [0]])

    # 内外参相乘
    temp_R_ll = np.append(R_ll, T_ll, axis=1)
    _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
    PL = np.dot(mtx_l, _temp_R_ll)

    # 右相机到左相机的投影矩阵
    temp_R_lr = np.append(R_lr, T_lr, axis=1)
    _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
    PR = np.dot(mtx_r, _temp_R_lr)

    point = np.array(cv2.triangulatePoints(PL, PR, uv_l, uv_r)).T
    wp_point = point[0][:-1] / np.max(point[0][-1])
    return wp_point

#三公共点坐标系转换算法
def calPoseFrom3Points(Oab, Pxb, Pyb):
    '''
    作者：xyq；
    日期：2022.1.18；
    功能：已知坐标系a的原点、x轴正半轴上任一点和y轴正半轴上任一点在坐标系b下的坐标，
        求解坐标系a到坐标系b的旋转矩阵R和平移矩阵T；
    输入：坐标系a的原点在坐标系b下的坐标:Oab(x1,y1,z1);
        坐标系a的x轴正半轴上任一点在坐标系b下的坐标:Pxb(x2,y2,z2);
        坐标系a的y轴正半轴上任一点在坐标系b下的坐标:Pyb(x3,y3,z3);
    输出：坐标系n到坐标系s的旋转矩阵Rns，输出格式为矩阵;
    DEMO：
            import geomeas as gm
            import numpy as np

            Oab = np.array([-37.84381632, 152.36389864, 41.68600167])
            Pxb = np.array([-19.59820338, 139.58818292, 45.55380309])
            Pyb = np.array([-38.23270656, 157.3130709, 59.86810327])

            print(gm.Pose().calPoseFrom3Points(Oab, Pxb, Pyb))
    '''
    x = (Pxb - Oab) / np.linalg.norm(Pxb - Oab)
    y = (Pyb - Oab) / np.linalg.norm(Pyb - Oab)
    z = np.cross(x, y)
    length = np.linalg.norm(z)
    z = z / length
    Rab = np.matrix([x, y, z]).transpose()
    Tab = np.matrix(Oab).transpose()
    return Rab, Tab

def calNewAnglesFrom2AnglesAndRotationMatrix(angle_hz, angle_v, R):
    '''
    作者:ljx;
    日期:2022.11.18;
    功能:已知一条直线l在坐标系1中的水平角、垂直角,以及坐标系1与坐标系2间的旋转矩阵R，求该直线在坐标系2中的水平角、垂直角;
    输入:直线l在坐标系1中的水平角angle_hz和垂直角angle_v，以及坐标系1与坐标系2间的旋转矩阵R
       （水平角为直线l在xz平面的投影线l‘与x轴的夹角,垂直角为直线l与xz平面的夹角）;
    输出:直线l在坐标系2中的水平角hz_new、垂直角v_new;
    DEMO:
            import numpy as np

            R = np.array([[8, 0, 640, 0],
                          [0, 8, 512, 0],
                          [0, 0, 1, 0]])  # 待校准标靶系与基准标靶系间旋转矩阵

            angle_hz = 87.06294733  #水平、垂直角
            angle_v = 25.78054998

            hz_new, v_new = calNewAnglesFrom2AnglesAndRotationMatrix(angle_hz, angle_v, R)
    '''
    hz = math.radians(angle_hz)
    v = math.radians(angle_v)

    x = math.cos(v) * math.cos(hz)
    y = math.sin(v)
    z = -math.cos(v) * math.sin(hz)

    PP = np.array([x, y, z])
    P = np.dot(PP, R.T).T

    x = P[0]
    y = P[1]
    z = P[2]

    hz_new = math.atan2(-z, x) * 180 / math.pi
    v_new = math.atan2(y, math.sqrt(x ** 2 + z ** 2)) * 180 / math.pi
    return hz_new, v_new



angle_hz = 87.06294733  #待校准工位轴线角度
angle_v = 25.78054998
hz_b = 74.87478168      #基准工位轴线角度
v_b = 40.70786725

Tb0_l = np.array([3.55550593 / 0.0053, 2.65832473 / 0.0053])  # 基准工位处光电标靶
Tb0_r = np.array([6.39255778 / 0.0053, 2.65832473 / 0.0053])
Tb1_l = np.array([3.14294457 / 0.0053, 2.65950158 / 0.0053])
Tb1_r = np.array([5.98056471 / 0.0053, 2.65950158 / 0.0053])
Tb2_l = np.array([3.55547749 / 0.0053, 2.32865551 / 0.0053])
Tb2_r = np.array([6.40594319 / 0.0053, 2.32865551 / 0.0053])
Tk0_l = np.array([1.95960674 / 0.0053, 1.67420817 / 0.0053])  # 待校准工位处光电标靶
Tk0_r = np.array([3.61379518 / 0.0053, 1.67420817 / 0.0053])
Tk1_l = np.array([1.71908251 / 0.0053, 1.67420817 / 0.0053])
Tk1_r = np.array([3.37327096 / 0.0053, 1.67420817 / 0.0053])
Tk2_l = np.array([1.95259578 / 0.0053, 1.47976580 / 0.0053])
Tk2_r = np.array([3.61488078 / 0.0053, 1.47976580 / 0.0053])

wp_Tb0 = cal3dCoordinateFromPixelCoordinates(Tb0_l, Tb0_r)
wp_Tb1 = cal3dCoordinateFromPixelCoordinates(Tb1_l, Tb1_r)
wp_Tb2 = cal3dCoordinateFromPixelCoordinates(Tb2_l, Tb2_r)
wp_Tk0 = cal3dCoordinateFromPixelCoordinates(Tk0_l, Tk0_r)
wp_Tk1 = cal3dCoordinateFromPixelCoordinates(Tk1_l, Tk1_r)
wp_Tk2 = cal3dCoordinateFromPixelCoordinates(Tk2_l, Tk2_r)

# 光电标靶坐标系与世界坐标系间旋转矩阵
Rw_tb = calPoseFrom3Points(wp_Tb0, wp_Tb1, wp_Tb2)[0]  # 基准标靶
Rw_tk = calPoseFrom3Points(wp_Tk0, wp_Tk1, wp_Tk2)[0]  # 待测标靶
Rtb_tk = np.dot(np.transpose(Rw_tb), Rw_tk)  # 待校准标靶系与基准标靶系间旋转矩阵

hz_new, v_new = calNewAnglesFrom2AnglesAndRotationMatrix(angle_hz, angle_v, Rtb_tk)

hz = hz_b - hz_new
v = v_b - v_new
print('水平角：', hz, '\n', '垂直角：', v)